#include "RegionGrowing.h"
#include "dialogs/RegionGrowingDialog.h"

#include <cc2sm.h>
#include <filtering.h>
#include <my_point_types.h>
#include <sm2cc.h>

#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>

//#include <pcl/segmentation/region_growing.h>


RegionGrowing::RegionGrowing()
	: BaseFilter(FilterDescription("Region Growing", "Segments pointcloud", "Segments pointcloud using smoothness constraints", ":/toolbar/PclUtils/icons/region_growing.png", true))
	, knn(10)
	, smoothMode(true)
	, smoothness_test(false)
	, curvature_test(false)
	, residual_test(false)
	, smoothness_treshold(0.0)
	, residual_treshold(0.0)
	, curvature_treshold(0.0)
	, curvature_percentil(0)
	, residual_percentil(0)
	, m_dialog(0)
	, min_cluster_size(1000)
	, max_cluster_size(1000000)
{

}

RegionGrowing::~RegionGrowing()
{
	if (m_dialog)
		delete m_dialog;
}

int RegionGrowing::openDialog()
{
	if (!m_dialog)
	{
		m_dialog = new RegionGrowingDialog;
	}

	ccPointCloud* cloud = getSelectedEntityAsCCPointCloud();
	//if (cloud)
	//{
	//	ccBBox bBox = cloud->getBB(true,false);
	//	if (bBox.isValid())
	//		m_dialog->radiusDoubleSpinBox->setValue(bBox.getDiagNorm() * 0.005);
	//}

	return m_dialog->exec() ? 1 : 0;
}


void RegionGrowing::getParametersFromDialog()
{
	assert(m_dialog);
	if (!m_dialog)
		return;
	
    //fill in parameters from dialog
	//general settings
	knn = m_dialog->spinBox_knn->value();
	smoothness_treshold = m_dialog->doubleSpinBox_smoothnessTreshold->value();
	smoothness_test = m_dialog->checkBox_smoothMode->checkState();

	min_cluster_size = m_dialog->spinBox_minClusterSize->value();
	max_cluster_size = m_dialog->spinBox_maxClusterSize->value();

	//residual test
	if(m_dialog->groupBox_residual->isChecked())
	{
		residual_test=true;
		if(m_dialog->checkBox_useResidualPercentil->checkState()){
			residual_percentil = m_dialog->spinBox_residualPercentil->value();
			residual_treshold = 0.0;
		}else{
			residual_percentil = 0;
			residual_treshold = m_dialog->doubleSpinBox_residualTreshold->value();	
		}

	}
	
	//curvature
	if(m_dialog->groupBox_curvature->isChecked())
	{
		curvature_test=true;
		if(m_dialog->checkBox_useCurvaturePercentil->checkState()){
			curvature_percentil = m_dialog->spinBox_curvaturePercentil->value();
			curvature_treshold = 0.0;
		}else{
			curvature_percentil = 0;
			curvature_treshold = m_dialog->doubleSpinBox_curvatureTreshold->value();	
		}
	}

}

int RegionGrowing::compute()
{
	
	//ccPointCloud* cloud = getSelectedEntityAsCCPointCloud();
	//if(!cloud)
	//	return -1;
	//
	//if(! cloud->hasNormals())
	//	return -14;

	//sensor_msgs::PointCloud2::Ptr sm_cloud (new sensor_msgs::PointCloud2);

	////get pointcloud in sensor_msgs format
 //   cc2smReader converter;
 //   converter.setInputCloud(cloud);
	//converter.getAsSM(*sm_cloud);

	//pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_cloud  (new pcl::PointCloud<pcl::PointXYZ>);
	//pcl::PointCloud<pcl::Normal>::Ptr pcl_cloud_normals (new pcl::PointCloud<pcl::Normal>);
	//pcl::fromROSMsg(*sm_cloud, *pcl_cloud_normals);

	//pcl::fromROSMsg(*sm_cloud,*pcl_cloud);

	//pcl::search::KdTree<pcl::PointXYZ>::Ptr tree  (new pcl::search::KdTree<pcl::PointXYZ>);

	//std::vector<float> residuals;
	//residuals.resize(pcl_cloud_normals->size());

	//for(int i=0;i< pcl_cloud_normals->size();i++)
	//{
	//	residuals[i] = pcl_cloud_normals->points[i].curvature;
	//}

	//std::sort(residuals.begin(),residuals.end());

	//float residual_per;
	//if(residual_test && residual_percentil!=0){
	//	
	//	float p_r = residuals.size() * ((float)residual_percentil / 100.0);
	//	
	//	//ganzzahlig?
	//	if(p_r==(int)p_r)
	//	{
	//		residual_per = 0.5 * ( residuals[p_r] + residuals[p_r+1]);
	//	}else
	//	{
	//		residual_per = residuals[ceil(p_r)];
	//	}

	//	residual_treshold=residual_per;
	//}
	//
	//float curvature_per;
	//if(curvature_test && curvature_percentil!=0){
	//	
	//	float p_r = residuals.size() * ((float)curvature_percentil / 100.0);
	//	
	//	//ganzzahlig?
	//	if(p_r==(int)p_r)
	//	{
	//		curvature_per = 0.5 * ( residuals[p_r] + residuals[p_r+1]);
	//	}else
	//	{
	//		curvature_per = residuals[ceil(p_r)];
	//	}

	//	curvature_treshold = curvature_per;
	//}
	//
	//pcl::RegionGrowing<pcl::PointXYZ,pcl::Normal> rG;
	//rG.setInputNormals(pcl_cloud_normals);
	//rG.setSearchMethod(tree);
	//rG.setInputCloud(pcl_cloud);
	//rG.setCurvatureTestFlag(curvature_test);

	//rG.setCurvatureThreshold(curvature_treshold);
	//rG.setSmoothnessThreshold(smoothness_treshold * M_PI / 180);
	//rG.setSmoothModeFlag(smoothMode);
	//rG.setResidualTestFlag(residual_test);
	//rG.setResidualThreshold(residual_treshold);
	//rG.setNumberOfNeighbours(knn);
	//rG.setMaxClusterSize(max_cluster_size);
	//rG.setMinClusterSize(min_cluster_size);
	//
	//std::vector <pcl::PointIndices> segments;
	//rG.extract(segments);
	//int num_of_segments = segments.size();
	//pcl::PointCloud<pcl::PointXYZRGB>::Ptr out_sm_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);	

	//sensor_msgs::PointCloud2::Ptr point_colored (new sensor_msgs::PointCloud2);
	//out_sm_cloud = rG.getColoredCloud();
	//pcl::toROSMsg(*out_sm_cloud, *point_colored);

	//sm2ccReader* converter2 = new sm2ccReader();
	//ccPointCloud* out_cloud = new ccPointCloud;
	//converter2->setInputCloud(point_colored);
	//int good = converter2->getAsCC(out_cloud);

	//ccHObject* cloud_container_large = new ccHObject();
	////ccHObject* cloud_container_small = new ccHObject();

	////int avg_cluster_size;
	////for(int i=0;i<num_of_segments;i++)
	////{
	////	avg_cluster_size+=segment_indices.at(i).size();
	////}
	////avg_cluster_size/=num_of_segments;
	////float treshold = 0.1;
	////for(int i=0;i<num_of_segments;i++)
	////{
	////	
	////	ccPointCloud *segment = new ccPointCloud();
	////	segment->reserveThePointsTable(segment_indices.at(i).size());

	////	for(int j =0; j< segment_indices.at(i).size();j++)
	////	{
	////		CCVector3* pt = new CCVector3();
	////		pt->x = pcl_cloud->at(segment_indices.at(i).at(j)).x;
	////		pt->y = pcl_cloud->at(segment_indices.at(i).at(j)).y;
	////		pt->z = pcl_cloud->at(segment_indices.at(i).at(j)).z;
	////		segment->addPoint(*pt);
	////	}

	////	if(segment_indices.at(i).size()<=treshold * avg_cluster_size)
	////		cloud_container_small->addChild(segment);
	////	else
	////		cloud_container_large->addChild(segment);
	////}

	////

	//cloud_container_large->addChild(out_cloud);
	////cloud_container_large->setName(qPrintable("Large Segments"));
	////cloud_container_small->setName(qPrintable("Small Segments"));
	//emit newEntity(cloud_container_large);
	//emit newEntity(cloud_container_small);
	return 1;

}
